package model.robot;

import model.virtualmap.OccupancyMap;

import java.io.PipedInputStream;
import java.io.BufferedReader;
import java.io.InputStreamReader;
import java.io.PrintWriter;
import java.io.PipedOutputStream;
import java.io.IOException;
import java.util.StringTokenizer;

/**
 * Title    :   The Mobile Robot Explorer Simulation Environment v2.0
 * Copyright:   GNU General Public License as published by the Free Software Foundation
 * Company  :   Hanze University of Applied Sciences
 *
 * @author Dustin Meijer        (2012)
 * @author Alexander Jeurissen  (2012)
 * @author Davide Brugali       (2002)
 * @version 2.0
 */

public class MobileRobotAI implements Runnable {

	private final OccupancyMap map;
	private final MobileRobot robot;

	private boolean running;

	public MobileRobotAI(MobileRobot robot, OccupancyMap map) {
		this.map = map;
		this.robot = robot;

	}

	/**
	 * In this method the gui.controller sends commands to the robot and its devices.
	 * At the moment all the commands are hardcoded.
	 * The exercise is to let the gui.controller make intelligent decisions based on
	 * what has been discovered so far. This information is contained in the OccupancyMap.
	 */
	public void run() {
		String result;
		this.running = true;
		double position[] = new double[3];
		double measures[] = new double[360];
		
		try {

			PipedInputStream pipeIn = new PipedInputStream();
			BufferedReader input = new BufferedReader(new InputStreamReader(pipeIn));
			PrintWriter output = new PrintWriter(new PipedOutputStream(pipeIn), true);

			robot.setOutput(output);
			System.out.println("intelligence running");
			
//      ases where a variable value is never used after its assignment, i.e.:
			
			int startAngle = 70;
			int stopAngle = 135;
			
			Integer corner;
			boolean onWall = false;
			
			int distanceToWall;
			int top;
			
			robot.sendCommand("R1.GETPOS");
			result = input.readLine();
			parsePosition(result, position);
			
			robot.sendCommand("L1.SCAN");
			result = input.readLine();
			parseMeasures(result, measures);
			map.drawLaserScan(position, measures);
			
			robot.sendCommand("P1.MOVEFW " + (measures[0]-10));
			result = input.readLine();
			
			robot.sendCommand("P1.ROTATELEFT 90");
			result = input.readLine();
			
			while (running && !isDone()) {

				robot.sendCommand("R1.GETPOS");
				result = input.readLine();
				parsePosition(result, position);
				System.out.println(result);
				
				robot.sendCommand("L1.SCAN");
				result = input.readLine();
				parseMeasures(result, measures);
				map.drawLaserScan(position, measures);
				
				top = (int)Math.round(measures[0]-10);
				corner = null;
				for(int i = stopAngle; i >= startAngle;i--)
				{
					double l = 30.0/Math.cos(Math.toRadians(i-90));
					System.out.println(i + " : " + l + " : " + measures[i]);
					if(Math.round(measures[i]) <= Math.round(l))
					{
						System.out.println("Hoek " + i + " : " + Math.ceil(measures[i]) + "/" + Math.ceil(l));
						double h = (Math.sqrt(Math.pow(l,2)-Math.pow(30,2)));
						corner = (int)Math.round(i < 90 ? h :-h);
					}
				}
				
				System.out.println("Corner: " + corner + " / Top: " + top);
				if(corner == null)
				{
					robot.sendCommand("P1.ROTATERIGHT 90");
					result = input.readLine();
				}
				else
				{
					if(top <= 0)
					{
						robot.sendCommand("P1.ROTATELEFT 90");
						result = input.readLine();
					}
					else if(top >= corner+60)
					{
						double move = corner+50;
						System.out.println("Move forward based on corner: " + move);
						robot.sendCommand("P1.MOVEFW " + (move));
						result = input.readLine();
					}
					else
					{
						robot.sendCommand("P1.MOVEFW " + (top < 50 ? top : 50));
						result = input.readLine();
					}
				}
			}
			System.err.println("Nothing left to discover.");
			
			this.running = false;
		} catch (IOException ioe) {
			System.err.println("execution stopped");
			running = false;
		}

	}

	private void parsePosition(String value, double position[]) {
		int indexInit;
		int indexEnd;
		String parameter;

		indexInit = value.indexOf("X=");
		parameter = value.substring(indexInit + 2);
		indexEnd = parameter.indexOf(' ');
		position[0] = Double.parseDouble(parameter.substring(0, indexEnd));

		indexInit = value.indexOf("Y=");
		parameter = value.substring(indexInit + 2);
		indexEnd = parameter.indexOf(' ');
		position[1] = Double.parseDouble(parameter.substring(0, indexEnd));

		indexInit = value.indexOf("DIR=");
		parameter = value.substring(indexInit + 4);
		position[2] = Double.parseDouble(parameter);
	}

	private void parseMeasures(String value, double measures[]) {
		//System.out.print(value);
		for (int i = 0; i < 360; i++) {
			measures[i] = 100.0;
		}
		if (value.length() >= 5) {
			value = value.substring(5);  // removes the "SCAN " keyword

			StringTokenizer tokenizer = new StringTokenizer(value, " ");

			double distance;
			int direction;
			while (tokenizer.hasMoreTokens()) {
				distance = Double.parseDouble(tokenizer.nextToken().substring(2));
				direction = (int) Math.round(Math.toDegrees(Double.parseDouble(tokenizer.nextToken().substring(2))));
				if (direction == 360) {
					direction = 0;
				}
				measures[direction] = distance;
				// Printing out all the degrees and what it encountered.
				//System.out.println("direction = " + direction + " distance = " + distance);
			}
		}
	}
	
	private boolean isDone()
	{
		char[][] m =  map.getGrid();
		int height = m[0].length;
		int width = m.length;
		int fieldCount = height * width;
		char field;
		System.out.println(height + ":" + width);
		
		for(int i = 0;i < fieldCount; i++)
		{
			int fx = i%width;
			int fy = i/width;
			field = m[fx][fy];
			
			if(field == map.getEmpty())
			{
				if(fx != 0)
					if(m[fx-1][fy] == map.getUnknown()) return false;
				if(fx != width-1)
					if(m[fx+1][fy] == map.getUnknown()) return false;
				if(fy != 0)
					if(m[fx][fy-1] == map.getUnknown()) return false;
				if(fy != height-1)
					if(m[fx][fy+1] == map.getUnknown()) return false;
			}
		}
		
		return true;
	}


}
